
#include <util/common_macros.h>

#include <imp/geometry/delaunay.h>
#include <imp/geometry/surface.h>

#include <imp/image/image.h>
#include <imp/image/morphology/immorph.h>
#include <imp/image/io/imageio.h>
#include <imp/image/pointop/threshold.h>
#include <imp/image/draw/draw.h>
#include <imp/image/features/features.h>
#include <imp/image/features/xcorner.h>
#include <imp/image/calibration/chessboard.h>
#include <imp/image/calibration/calibration.h>

#include <imp/dsp/filters/filters.h>
#include <imp/dsp/primitives.h>

#include <imp/stat/stat_primitives.h>

#include "camcal.h"
// =============================================================================

#define __proc proc

// =============================================================================

enum _states {
	START,
	GRAB,
	CAP_GRID,
	GRID_IS_VALID,
	SAVE,
	SHOW_RAW,
	SHOW_CAP,
	EXIT,
	N_STATES
};

static void proc_hess(CamCal * cc) {
	ImpMatrix * Mat = NULL;
	ImpMatrix * Gauss = NULL;
	ImpMatrix * MatConv = NULL;
	ImpMatrix * BW = NULL;
	ImpMatrix * SE = NULL;
	ImpMatrix * Dilate = NULL;
	ImpMatrix * Erode = NULL;
	ImpMatrix * Edge = NULL;
	ImpMatrix * Hess = NULL;
	ImpMatrix * NMS = NULL;

	imp_image_gray2mat(cc->image_gray, &Mat);
	Gauss = imp_filter_new_gaussian(3, 3 / 4.0);
	imp_dsp_conv(Mat, Gauss, &MatConv);

	imp_image_features_hessian_response(MatConv, &Hess);

	int id;
	real max;
	__vmax(Hess->size, Hess->data, 1, &max, &id);

	imp_matrix_normailze(Hess, max, 0.0);
	imp_image_nonmaxsup_feature(Hess, 3, NULL, &NMS);

	if (max > 0) {
		imp_image_threshold(NMS, max * 0.8, 0.0, max, IMP_THRESHOLD_BINARY, NULL);
	}

	imp_image_mat2rgb(NMS, &cc->image_rgb);

	imp_object_delete(Mat);
	imp_object_delete(Gauss);
	imp_object_delete(MatConv);
	imp_object_delete(BW);
	imp_object_delete(SE);
	imp_object_delete(Dilate);
	imp_object_delete(Erode);
	imp_object_delete(Edge);
	imp_object_delete(Hess);
	imp_object_delete(NMS);
}

static void proc(CamCal * cc) {

	char im_name[32];
	int ret;

	GtkTreeIter iter;
	ImpImage * Img_resp = NULL;
	ImpMatrix * X = NULL;
	ImpMatrix * Xi = NULL;

	ImpMatrix * H = NULL;
	ImpMatrix * Hi = NULL;

	ImpMatrix * Mat = NULL;
	
	int i;

	enum _states state = START;

	while (state != EXIT) {

		switch (state) {
			case START:
			case GRAB:

				if (cc->ct_capture) state = CAP_GRID;
				else state = EXIT;
				break;

			case CAP_GRID: //===================================================

				imp_image_gray2mat(cc->image_gray, &Mat);
				imp_image_blur(Mat, 3);
				
				ret = chess_detect(Mat, 11, 7, &Xi, &X);

				//ret = imp_image_calib_find_partial_chessboard(Mat, &X, &Xi);

				if (!ret) {
					state = EXIT;
					imp_object_delete(Mat);
					break;
				}

				//imp_image_mat2rgb(Mat, &cc->image_rgb);
				imp_image_chessboard_draw_grid(cc->image_rgb, X, Xi, TRUE);

				nDLT(X, Xi, &H);
				imp_object_delete(Mat);

/*
				sXi = get_search_grid(X, 11, 7, H);

				//if (sXi->columns == 77) {
					ImpDraw draw = imp_draw_create(cc->image_rgb);
					draw.fg = imp_color(255, 0, 0);
					for (i = 0; i < sXi->columns; i++) {
						imp_draw_circle(&draw, sXi->data[i], sXi->data[i + sXi->columns], 3);
						imp_draw_circle(&draw, sXi->data[i], sXi->data[i + sXi->columns], 4);
					}
					imp_object_delete(sXi);
				//}
*/

				if (cc->ct_image_count >= cc->ct_n_images) {
					cc->ct_capture = FALSE;
					gtk_toggle_button_set_active(cc->btg_capture, FALSE);
					state = EXIT;
					break;
				}

				if (Xi->columns > cc->ct_x * cc->ct_y) {
					imp_object_delete(X);
					imp_object_delete(Xi);
					imp_object_delete(H);
					state = EXIT;
					break;
				}

				if (cc->ct_frame_count < cc->ct_frame_step) {
					cc->ct_frame_count++;
					imp_object_delete(X);
					imp_object_delete(Xi);
					imp_object_delete(H);
					state = EXIT;
					break;
				}

				state = SAVE;
				break;

			case SAVE: //=======================================================

				sprintf(im_name, "image %d", cc->ct_image_count_name);

				cc->ct_image_count++;
				cc->ct_image_count_name++;

				cc->ct_frame_count = 0;


				// Create image to store
				imp_image_copy(cc->image_rgb, &Img_resp);

				gtk_list_store_append(cc->lstore_images, &iter);
				gtk_list_store_set(cc->lstore_images, &iter, 0, im_name, 1, Xi->columns, 2, Img_resp, 3, X, 4, Xi, -1);

				for (i = 0; i < cc->image_rgb->nbytes; i++) {
					cc->image_rgb->data[i] = 255 - cc->image_rgb->data[i];
				}

				state = EXIT;

				break;

			default:
				state = EXIT;
				break;
		}
	}

	ImpDraw draw = imp_draw_create(cc->image_rgb);
	draw.fg = imp_color(255, 255, 0);
	draw.bg = imp_color(100, 60, 120);
	imp_draw_text(&draw, 5, 5, TRUE, "> Step: %d ", cc->ct_frame_step - cc->ct_frame_count);
	imp_draw_text(&draw, 5, 17, TRUE, "> Images: %d ", cc->ct_n_images - cc->ct_image_count);

}

static gboolean __loop(CamCal * cc) {

	char * buff;
	buff = v4l_interface_grab_rgb(cc->v4li, cc->image_rgb->data);
	imp_image_rgb2gray(cc->image_rgb, &cc->image_gray);

	__proc(cc);

	gtk_widget_queue_draw(GTK_WIDGET(cc->da_image));

	return TRUE;
}

static void __alloc_work_memory(CamCal * cc) {

	if (cc->image_rgb) imp_image_delete(cc->image_rgb);
	if (cc->image_gray) imp_image_delete(cc->image_gray);

	cc->image_rgb = imp_image_new(cc->v4li->v4ldrv.width, cc->v4li->v4ldrv.height, IMP_IMAGE_TYPE_RGB);
	cc->image_gray = imp_image_new(cc->v4li->v4ldrv.width, cc->v4li->v4ldrv.height, IMP_IMAGE_TYPE_GRAY);

}

static void __event_v4li_start(v4lInterface * v4li, gpointer data) {

	CamCal * cc = (CamCal *) data;

	gtk_widget_set_size_request(GTK_WIDGET(cc->da_image), cc->v4li->v4ldrv.width, cc->v4li->v4ldrv.height);

	__alloc_work_memory(cc);

	//start loop
	cc->ct_capture = FALSE;
	gtk_toggle_button_set_active(cc->btg_capture, FALSE);


	gtk_widget_set_sensitive(GTK_WIDGET(cc->treeview), FALSE);
	gtk_widget_set_sensitive(GTK_WIDGET(cc->btg_capture), TRUE);

	cc->handle_loop = g_idle_add((GSourceFunc) __loop, cc);
}

static void __event_v4li_stop(v4lInterface * v4li, gpointer data) {

	CamCal * cc = (CamCal *) data;

	if (cc->handle_loop == -1) return;

	gtk_widget_set_sensitive(GTK_WIDGET(cc->treeview), TRUE);
	gtk_widget_set_sensitive(GTK_WIDGET(cc->btg_capture), FALSE);

	cc->ct_capture = FALSE;
	gtk_toggle_button_set_active(cc->btg_capture, FALSE);

	// stop loop
	g_source_remove(cc->handle_loop);
	cc->handle_loop = -1;
}

static void __enable_widgets(CamCal * cc, gboolean enable) {

	gtk_widget_set_sensitive(GTK_WIDGET(cc->spb_frame_step), enable);
	gtk_widget_set_sensitive(GTK_WIDGET(cc->spb_n_images), enable);
	gtk_widget_set_sensitive(GTK_WIDGET(cc->spb_square_size), enable);
	gtk_widget_set_sensitive(GTK_WIDGET(cc->spb_vertex_ny), enable);
	gtk_widget_set_sensitive(GTK_WIDGET(cc->spb_vertex_nx), enable);

}

static void __event_btg_capture(GtkWidget * widget, gpointer data) {

	CamCal * cc = (CamCal *) data;
	cc->ct_capture = gtk_toggle_button_get_active(cc->btg_capture);

	if (cc->ct_capture) {
		cc->ct_x = (int) gtk_spin_button_get_value(cc->spb_vertex_nx);
		cc->ct_y = (int) gtk_spin_button_get_value(cc->spb_vertex_ny);
		cc->ct_square_size = (float) gtk_spin_button_get_value(cc->spb_square_size);
		cc->ct_n_images = (int) gtk_spin_button_get_value(cc->spb_n_images);
		cc->ct_frame_step = (int) gtk_spin_button_get_value(cc->spb_frame_step);
	}

	__enable_widgets(cc, !cc->ct_capture);

}

static gboolean __event_treeview_select(GtkWidget * widget, gpointer data) {

	CamCal * cc = (CamCal *) data;
	GtkTreeView * tree_view = GTK_TREE_VIEW(widget);

	GtkTreeSelection * sel = gtk_tree_view_get_selection(tree_view);

	GtkTreeModel * model;
	GtkTreeIter iter;
	if (gtk_tree_selection_get_selected(sel, &model, &iter)) {
		ImpImage * Img;
		gtk_tree_model_get(model, &iter, 2, &Img, -1);

		imp_image_copy(Img, &cc->image_rgb);
		gtk_widget_queue_draw(GTK_WIDGET(cc->da_image));
	}
	return FALSE;
}

#include <gdk/gdkkeysyms.h>

static gboolean __event_treeview_key(GtkWidget * widget, GdkEvent * event, gpointer data) {

	if (event->key.keyval != GDK_KEY_Delete) return FALSE;

	//printf("%i\n", event->key.keyval);
	//return FALSE;

	CamCal * cc = (CamCal *) data;
	GtkTreeView * tree_view = GTK_TREE_VIEW(widget);

	GtkTreeSelection * sel = gtk_tree_view_get_selection(tree_view);

	GtkTreeModel * model;
	GtkTreeIter iter;
	if (gtk_tree_selection_get_selected(sel, &model, &iter)) {

		ImpImage * Img = NULL;
		ImpMatrix * Mat = NULL;

		gtk_tree_model_get(model, &iter, 2, &Img, 3, &Mat, -1);

		imp_object_delete(Img);
		imp_object_delete(Mat);

		gtk_list_store_remove(GTK_LIST_STORE(model), &iter);

		cc->ct_image_count--;
	}

	return FALSE;

}

static void __event_bt_clear(GtkButton * button, gpointer data) {
	CamCal * cc = (CamCal *) data;

	GtkTreeModel * model = gtk_tree_view_get_model(cc->treeview);
	GtkTreeIter iter;

	if (!gtk_tree_model_get_iter_first(model, &iter)) return;

	do {

		ImpImage * Img = NULL;
		ImpMatrix * Mat = NULL;

		gtk_tree_model_get(model, &iter, 2, &Img, 3, &Mat, -1);

		imp_object_delete(Img);
		imp_object_delete(Mat);

	} while (gtk_tree_model_iter_next(model, &iter));

	gtk_list_store_clear(GTK_LIST_STORE(model));

	cc->ct_image_count = 0;

}

static void __event_bt_calibre(GtkButton * button, gpointer data) {
	CamCal * cc = (CamCal *) data;


}


/* ========================================================================== */

/* ========================================================================== */
static gboolean __event_expose_da_image(GtkWidget * drawarea, GdkEvent * e, gpointer data) {

	CamCal * cc;
	cc = (CamCal *) data;

	GtkWidget * widget = GTK_WIDGET(drawarea);
	GdkWindow * window = gtk_widget_get_window(GTK_WIDGET(widget));
	ImpImage * Img = NULL;

	// Select image
	Img = cc->image_rgb;

	if (Img == NULL) return FALSE;
	if (Img->data == NULL) return FALSE;

	gdk_draw_rgb_image(window, widget->style->fg_gc[GTK_STATE_NORMAL],
			0, 0, Img->width, Img->height,
			GDK_RGB_DITHER_MAX, Img->data, Img->stride);

	return FALSE;
}

/* ========================================================================== */
void __camcal_init_events(CamCal * cc) {

	int i;
	gtk_widget_set_size_request(GTK_WIDGET(cc->da_image), 320, 240);
	g_signal_connect(G_OBJECT(cc->da_image), "expose-event", G_CALLBACK(__event_expose_da_image), cc);

	g_signal_connect(G_OBJECT(cc->v4li), "v4li-start-event", G_CALLBACK(__event_v4li_start), cc);
	g_signal_connect(G_OBJECT(cc->v4li), "v4li-stop-event", G_CALLBACK(__event_v4li_stop), cc);

	g_signal_connect(G_OBJECT(cc->btg_capture), "toggled", G_CALLBACK(__event_btg_capture), cc);

	g_signal_connect(G_OBJECT(cc->treeview), "cursor-changed", G_CALLBACK(__event_treeview_select), cc);

	g_signal_connect(G_OBJECT(cc->treeview), "key-release-event", G_CALLBACK(__event_treeview_key), cc);

	g_signal_connect(G_OBJECT(cc->bt_clear), "clicked", G_CALLBACK(__event_bt_clear), cc);

	g_signal_connect(G_OBJECT(cc->bt_calibre), "clicked", G_CALLBACK(__event_bt_calibre), cc);

}
